import java.io.File;
import java.io.FileNotFoundException;
import java.io.PrintWriter;
import java.util.ArrayList;
import java.util.Observable;
import java.util.Scanner;

import com.mobilerobots.Aria.ArAction;
import com.mobilerobots.Aria.ArActionDesired;
import com.mobilerobots.Aria.ArActionStallRecover;
import com.mobilerobots.Aria.ArArgumentParser;
import com.mobilerobots.Aria.ArLaser;
import com.mobilerobots.Aria.ArLaserConnector;
import com.mobilerobots.Aria.ArLog;
import com.mobilerobots.Aria.ArPose;
import com.mobilerobots.Aria.ArRangeDevice;
import com.mobilerobots.Aria.ArRobot;
import com.mobilerobots.Aria.ArRobotConnector;
import com.mobilerobots.Aria.ArSimpleConnector;
import com.mobilerobots.Aria.ArSonarDevice;
import com.mobilerobots.Aria.Aria;

/*
MobileRobots Advanced Robotics Interface for Applications (ARIA)
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
Copyright (C) 2006, 2007 MobileRobots Inc.

     This program is free software; you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
     the Free Software Foundation; either version 2 of the License, or
     (at your option) any later version.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program; if not, write to the Free Software
     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

If you wish to redistribute ARIA under different terms, contact 
MobileRobots for information about a commercial version of ARIA at 
robots@mobilerobots.com or 
MobileRobots Inc, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481
*/


/* 
 
example program demonstrating how to make and use new actions.

This example program creates two new actions, Go and Turn. Go will drive the robot forward safely,
while Turn will avoid obstacles detected by the sonar by turning. 
This program also adds a predefined
action from Aria which tries to recover from stalls (hit something and 
can't move forward) by backing and turning.

Each of these actions have the normal constructor and destructor, note that 
the constructors use constructor chaining to create their ArAction part
correctly.  Each action then also implements the essential virtual method, 
fire(). This fire function is called by the action resolver, and
returns values that, in combination with other actions' desired behavior,
determine the driving commands sent to the robot.

Also note that each of these actions override the setRobot function; these 
implementations obtain the sonar device from the robot in addition to doing the
needed caching of the robot pointer.  This is what you should do if you
care about the presence or absence of a particular sensor.  If you don't
care about any particular sensor you could just use one of the  checkRangeDevice...
methods in ArRobot (there are four of them).
Also note that these are very naive actions, they are simply an example
of how to use actions.

See the @ref actions Actions section of the Aria reference manual overview for more details about actions.

Note that actions must take a small amount of time to execute, to avoid
delaying the robot synchronization cycle.

*/

class EsquivarAction extends ArAction
{
	
	private ArActionDesired myDesired;
	


	protected ArRangeDevice myRangeDevice;
	
	private double dMin;
	
	private int randomed;
	
	public EsquivarAction(double dMin){
		super("Esquivar");
		this.dMin = dMin;
		myDesired = new ArActionDesired();
		randomed = 0;
	}
	
	public ArActionDesired fire(ArActionDesired currentDesired)
	 {
		try{
			double frontReading = myRangeDevice.currentReadingPolar(-75, 75)-400;
			double closeRightReading = myRangeDevice.currentReadingPolar(-45, -30)-400;
			double closeLeftReading = myRangeDevice.currentReadingPolar(30, 45)-400;
			double farRightReading = myRangeDevice.currentReadingPolar(-60, -45)-400;
			double farLeftReading = myRangeDevice.currentReadingPolar(45, 60)-400;
			double farfarRightReading = myRangeDevice.currentReadingPolar(-80, -60)-400;
			double farfarLeftReading = myRangeDevice.currentReadingPolar(60, 80)-400;
			myDesired.reset();
			
			//System.out.printf("My readings %s, %s, %s, %s, %s \n", farLeftReading, closeLeftReading, frontReading, closeRightReading, farRightReading);
			if(frontReading > dMin*2){
				ArPose pose = getRobot().getPose();
				
				 
				//No hay necesidad de esquivar
				return myDesired;
			}else{
				double closeD =dMin/Math.cos(30*Math.PI/180)+20;
				double farD = dMin/Math.cos(45*Math.PI/180)+20;
				double farfarD = dMin/Math.cos(60*Math.PI/180)+20;
				if(closeRightReading < closeD){
					//System.out.println("Avoiding close right");
					myDesired.setDeltaHeading(10);
					randomed = 0;
				}else if (closeLeftReading < closeD){
					//System.out.println("Avoiding close left");
					myDesired.setDeltaHeading(-10);
					randomed = 0;
				}else if (farRightReading < farD){
					//System.out.println("Avoiding far right");
					myDesired.setDeltaHeading(15);
					randomed = 0;
				}else if(farLeftReading < farD){
					//System.out.println("Avoiding far left");
					myDesired.setDeltaHeading(-15);
					randomed = 0;
				}else if (farfarRightReading < farfarD){
					//System.out.println("Avoiding far far right");
					myDesired.setDeltaHeading(20);
					randomed = 0;
				}else if(farfarLeftReading < farfarD){
					//System.out.println("Avoiding far far left");
					myDesired.setDeltaHeading(-20);
					randomed = 0;
				}else{
					if(randomed==0){					
						int s = (int)(Math.random()*2);
						if(s==0){
							s=-1;
						}
						randomed = s;
					}
					myDesired.setDeltaHeading(15*randomed);
				//	System.out.println("Avoiding at random directon");
					
				}
				//myDesired.setVel(50);
			}
		}catch (Exception e) {
			e.printStackTrace();
		}
			
		return myDesired;
	 }
	public void setRobot(ArRobot robot)
	{
		setActionRobot(robot);
		ArRangeDevice laser_urg1 = robot.findRangeDevice("urg_1");
		ArRangeDevice laser_sim_urg1= robot.findRangeDevice("sim_urg_1");
		ArRangeDevice sonar = robot.findRangeDevice("sonar");
		
		if(laser_urg1!=null)
		    myRangeDevice = laser_urg1;
		else if(laser_sim_urg1!=null)
			myRangeDevice = laser_sim_urg1;
		else
			myRangeDevice = sonar;
	}
	

}

class AvanzarAction extends ArAction
{
	private double dVisita;
	
	private double dMin;
	
	private ArActionDesired myDesired;

	protected ArRangeDevice myRangeDevice;
	
	
	private ArrayList<Point> points;
	
	private Point lastVisited;
	
	private double traveledDistance;
	
	private ArrayList<Point> savedPoints;
	
	private int counter;
	
	private String outMap;
	
	private String outData;
	
	private boolean firstWritting;
	
	public AvanzarAction(double dVisita, double dMin, ArrayList<Point> points, String outMapFile, String outDataFile){
		super("Avanzar");
		outMap = outMapFile;
		outData = outDataFile;
		this.dVisita = dVisita;
		this.dMin = dMin;
		this.points = points;
		myDesired = new ArActionDesired();
		traveledDistance=0;
		counter=0;
		savedPoints=new ArrayList();
	}
	
	public ArActionDesired fire(ArActionDesired currentDesired)
	 {
		try{
			myDesired.reset();
			//System.out.println("punto debug 0 referencia myLaser: "+ myLaser);
			double frontReading = myRangeDevice.currentReadingPolar(-30, 30);
			//System.out.println("punto debug 1 referencia fronReading: "+ frontReading);
			ArPose pose = getRobot().getPose();
			//System.out.println("x: "+pose.getX()+ "y:" +pose.getY());
			if(lastVisited !=null){
				traveledDistance+= lastVisited.distance(pose.getX(), pose.getY());			
			}
			lastVisited = new Point(pose.getX(),pose.getY());
			//System.out.println("punto debug 2");
			//Revisa si el punto que esta buscando esta cerca (a menos de la distancia minima de visita
			//System.out.println(new Point(pose.getX(),pose.getY()));
			if(Proyecto1.puntoActual!=null && Proyecto1.puntoActual.withinRange(pose.getX(), pose.getY(), dVisita)){
				System.out.println("Recorrido:"+traveledDistance);
				
				Proyecto1.agregarMensajeConsola("Se encontro el punto "+Proyecto1.puntoActual.getPosX()+","+Proyecto1.puntoActual.getPosY());
				
				Proyecto1.disReal+=traveledDistance;
				points.remove(0);
				if(points.size()>0){
					Proyecto1.puntoActual = points.get(0);
				}else{
					Proyecto1.puntoActual=null;
					Proyecto1.agregarMensajeConsola("Se finalizo el recorrido!");
					Proyecto1.agregarMensajeConsola("Generando el mapa");
					try{
						PrintWriter pw = new PrintWriter(new File(outMap));
						pw.println();
						pw.println("2D-Map");
						pw.println("Cairn: RobotHome 0 0 0 \"\" ICON \"q0\"");
						pw.println("DATA");
						for (Point p : savedPoints) {
							pw.println((int)p.getPosX()+" "+(int)p.getPosY());
						}
						pw.close();
						Proyecto1.agregarMensajeConsola("Archivo de localizacion generado en: "+outMap);
					}catch (Exception e) {
						System.out.println("Error writing file");
						Proyecto1.agregarMensajeConsola("Error escribiendo el mapa");
					}
				}
			}
			try{
			// Revisa si hay algun obstaculo al frente y avanza en caso de tener via libre y puntos por visitar
			if((frontReading-400) < dMin ){
				myDesired.setVel(0);			
			}
			else if(Proyecto1.puntoActual==null)
			{
				if(firstWritting){
					firstWritting = false;
					myDesired.setVel(0);
					Proyecto1.posFinalReal = lastVisited;
					PrintWriter pw;
					try {
						Proyecto1.agregarMensajeConsola("Generando archivo de informacion: "+outData);
						pw = new PrintWriter(new File(outData));
						pw.println();
						pw.println("Distancia Nominal: "+ Proyecto1.disNominal);
						pw.println("Distancia Real: "+ Proyecto1.disReal);
						pw.println("Posici�n Final Nominal: "+ Proyecto1.posFinalNominal);
						pw.println("Posici�n Final Real: "+ Proyecto1.posFinalReal);
						double difPosicion =Proyecto1.posFinalNominal.distance(Proyecto1.posFinalReal.getPosX(), Proyecto1.posFinalReal.getPosY());
						pw.println("Diferencia Posicion: "+ difPosicion);
						double porcentajeErrorDistancias = ((Proyecto1.disNominal-Proyecto1.disReal)*100)/Proyecto1.disNominal;
						double porcentajeErrorPosicion = (difPosicion/Proyecto1.disNominal)*100;
						pw.println("Porcentaje Error Distancia: " +Math.abs(porcentajeErrorDistancias)+"%");
						pw.println("Porcentaje Error Posicion: " + Math.abs(porcentajeErrorPosicion)+"%");
						pw.close();
						Proyecto1.agregarMensajeConsola("Archivo de informacion generado en: "+outData);
					} catch (FileNotFoundException e) {
						// TODO Auto-generated catch block
						Proyecto1.agregarMensajeConsola("Error generando archivo de informacion");
						System.out.println(e.getMessage());
					}
				}
	
				
			}else{
				myDesired.setVel(100);
			}
			}
			catch(Exception e)
			{
				e.printStackTrace();
			}
			
			saveReadings(pose);
		}catch (Exception e) {
			e.printStackTrace();
		}
		//	myDesired.setVel(50)
			return myDesired;
	 }
	
	private void saveReadings(ArPose ar){
		double heading = ar.getTh();
		//heading+=360;
		//heading = heading % 360;
		ArPose pose = getRobot().getPose();
		for (int i = -180; i < 180; i+=12) {
			double d = myRangeDevice.currentReadingPolar(i-1, i+1);
			double ang = (heading+i)*Math.PI/180;
			Point p = new Point(pose.getX()+ Math.cos(ang)*d, pose.getY()+Math.sin(ang)*d);
			savedPoints.add(p);
			
		}
		
	}
	public void setRobot(ArRobot robot)
	{
		setActionRobot(robot);
		ArRangeDevice laser_urg1 = robot.findRangeDevice("urg_1");
		ArRangeDevice laser_sim_urg1= robot.findRangeDevice("sim_urg_1");
		ArRangeDevice sonar = robot.findRangeDevice("sonar");
		
		if(laser_urg1!=null)
		    myRangeDevice = laser_urg1;
		else if(laser_sim_urg1!=null)
			myRangeDevice = laser_sim_urg1;
		else
			myRangeDevice = sonar;
	}

	  
	
}

class OrientarAction extends ArAction
{
	private ArActionDesired myDesired;
	
	private double tolerance;
	
	private double finalAngle;

	private double dMin;

	
	protected ArRangeDevice myRangeDevice;
	
	public OrientarAction(double tolerance, double finalAngle, double dMin){
		super("Orientar");
		myDesired = new ArActionDesired();
		this.tolerance = tolerance;
		this.finalAngle = finalAngle;
		this.dMin = dMin;
	}
	
	 public ArActionDesired fire(ArActionDesired currentDesired)
	 {
		 try{
			 myDesired.reset();
			 /*
			 if(myDesired.getVel()==0)
				 myDesired.setVel(100);
			 else
				 myDesired.setVel(0);
			 */
			double rightReading = myRangeDevice.currentReadingPolar(-90, 0)-400;
			double leftReading = myRangeDevice.currentReadingPolar(0, 90)-400;
			 double desiredAngle=0,currentAngle=0;
			 ArPose pose = getRobot().getPose();
			 currentAngle = pose.getThRad();
			 if (Proyecto1.puntoActual!=null){
				 double dy = Proyecto1.puntoActual.getPosY()-pose.getY();
				 double dx = Proyecto1.puntoActual.getPosX()-pose.getX();
				 desiredAngle = Math.atan(Math.abs(dy)/Math.abs(dx));
				 
				 if(dx<0 && dy<0){
					 desiredAngle= desiredAngle-Math.PI;
				 }else if(dy<0){
					 desiredAngle*=-1;
				 }else if(dx<0){
					 desiredAngle= desiredAngle-Math.PI;
					 desiredAngle*=-1;
				 }
				 
				 
				 if(Math.abs(desiredAngle-currentAngle) > tolerance){
					 /*
					 if(Math.abs(deltaAng) > Math.PI){
						 if(deltaAng>0){
							 deltaAng-=Math.PI;
							 deltaAng*=-1;
						 }else{
							 deltaAng+=Math.PI;
							 deltaAng*=-1;
						 }
					 }
					 deltaAng = deltaAng*180/Math.PI;
					 
					 desiredAngle = desiredAngle*180/Math.PI;
					 if(desiredAngle > 180){
						desiredAngle-=180;
						desiredAngle*=-1;
					 }
					 if(desiredAngle>0)
					 {
						 if(leftReading<dMin)
							 desiredAngle=0;					
					 }
					 else
					 {
						 if(rightReading<dMin)						 
							 desiredAngle=0;		 
						
					 }*/
					 if(leftReading> dMin && rightReading > dMin){
						 myDesired.setHeading(desiredAngle*180/Math.PI);
					 }
					 
					 
					 
				 }else{
					 myDesired.setDeltaHeading(0);
				 }
			 }else{
				 if(currentDesired.getDeltaHeading()==0){				 
					 myDesired.setHeading((finalAngle),1);
				 }
			 }
		 }catch (Exception e) {
			e.printStackTrace();
		}
		 return myDesired;
	 }
	 
		public void setRobot(ArRobot robot)
		{
			setActionRobot(robot);
			ArRangeDevice laser_urg1 = robot.findRangeDevice("urg_1");
			ArRangeDevice laser_sim_urg1= robot.findRangeDevice("sim_urg_1");
			ArRangeDevice sonar = robot.findRangeDevice("sonar");
			
			if(laser_urg1!=null)
			    myRangeDevice = laser_urg1;
			else if(laser_sim_urg1!=null)
				myRangeDevice = laser_sim_urg1;
			else
				myRangeDevice = sonar;
		}
}




/** 
 * Example Action class that drives the robot forward, but stops if obstacles are
 * detected by sonar. 
 */




public class Proyecto1 extends Observable{



	
  /* Try to load the Aria and ArNetworking wrapper libraries when the
   * program starts:
   */
		static String robotName;
	  static Point puntoActual;
	  private static Proyecto1 instance;
	  public static int disNominal=0;
	  public static int disReal=0;
	  public static Point posFinalNominal;
	  public static Point posFinalReal;
	  
	
  static {
    try {
      System.loadLibrary("AriaJava");
      
    } catch(UnsatisfiedLinkError e) {
      System.err.println("Native code library libAriaJava failed to load. Make sure that its directory is in your system library path. See the chapter on Dynamic Linking Problems in the SWIG Java Documentation (http://www.swig.org) for help.\n" + e);
      System.exit(1);
    }
  }

  
  public static void main(String[] args) {
		InterfazProyecto inter = new InterfazProyecto(args);
		inter.setVisible(true);
		  //run(args, "./problema2-mundo1/problema2-mundo1-7x17-puntos.txt");

	}
  

  public static void run(String argv[],String mundo) {
	  double dVisita = 0,dMin= 0,finalAngle = 0;
	  ArrayList<Point> puntos = new ArrayList<Point>();
	  //String mundo = mundo3;
	  System.out.println(mundo);
	  try{
		  //Lectura del archivo de config 
		  Scanner sc = new Scanner(new File(mundo));
		  dVisita = sc.nextDouble();
		  dMin = sc.nextDouble();
		  int n = sc.nextInt();
		  for (int i = 0; i < n; i++) {
			  Point punto = new Point(sc.nextDouble(),sc.nextDouble());
			  puntos.add(punto);
		  }
		  puntoActual = puntos.get(0);
		  finalAngle = sc.nextDouble();
	  }catch (Exception e) {
		  e.printStackTrace();
		  Proyecto1.agregarMensajeConsola("Error leyendo el archivo de configuracion");
		  System.out.println("Error leyendo el archivo de configuracion");
	}
	
	  
	  darDistanciaNominal(puntos);
	  posFinalNominal = puntos.get(puntos.size()-1);
	  boolean one = false;
	  for (int i = 0; i < argv.length && !one; i++) {
		   one = (argv[i].equals("8102"));
	   } 
	  String outMapFile = mundo.substring(0,23);
	  if(!one){
		  outMapFile+="-laser-A.map";
	  }else{
		  outMapFile+="-laser-B.map";
	  }
	  
	  String outDataFile = mundo.substring(0,16);
	  outDataFile+="-datos-de-salida.txt";
	  
	  
	    Aria.init();

   

	    //ArSimpleConnector conn = new ArSimpleConnector(argv);
	    ArRobot robot = new ArRobot();
	   // ArSonarDevice sonar = new ArSonarDevice();

	    
	    ArArgumentParser parser = new ArArgumentParser(argv);
	    //parser.loadDefaultArguments();
	    

	    ArRobotConnector robotConn = new ArRobotConnector(parser, robot);
	    
	    
	    if(!robotConn.connectRobot(robot))
	    {
	      ArLog.log(ArLog.LogLevel.Terse, "actionExample: Could not connect to robot! Exiting.");
	      if(parser.checkHelpAndWarnUnparsed())
	      {
	          // -help not given, just exit.
	          Aria.logOptions();
	          Aria.exit(1);
	      }

	    }
	    
	    
	    
	    
	    
	   
	    //laserConn.connectLasers(true);

	    // Create instances of the actions defined above, plus ArActionStallRecover, 


	    OrientarAction orientar = new OrientarAction(0.2,finalAngle,dMin);
	    
	    AvanzarAction avanzar = new AvanzarAction(dVisita,dMin,puntos,outMapFile, outDataFile);
	    EsquivarAction esquivar = new EsquivarAction(dMin);
	    // a predefined action from Aria.
	    ArActionStallRecover recover = new ArActionStallRecover();

	      
	    
	    // Parse all command-line arguments
	    if(!Aria.parseArgs())
	    {
	      Aria.logOptions();
	      System.exit(1);
	    }
	    
	    // Connect to the robot
//	    if(!conn.connectRobot(robot))
//	    {
//	      ArLog.log(ArLog.LogLevel.Terse, "actionExample: Could not connect to robot! Exiting.");
//	      System.exit(2);
//	    }
	   // parser.addDefaultArgument("-connectLaser");


	    
	 


	    // Add the range device to the robot. You should add all the range 
	    // devices and such before you add actions
	   // robot.addRangeDevice(sonar);
	    boolean hasLaser = false;
	   for (int i = 0; i < argv.length && !hasLaser; i++) {
		   hasLaser = (argv[i].equals("-laserType"));
	   } 
	    
	    if(hasLaser){
	    	ArLaserConnector laserConn = new ArLaserConnector(parser, robot, robotConn);
	    	
	    	laserConn.connectLasers(false, false, true);
	    	
	    	ArLaser laser = null;
	    	
	    	for(int i =0; i<100 && laser==null; i++)
	    	{
	    		laser = robot.findLaser(i);
	    		//System.out.println(las);
	    		if(laser!=null)
	    		{
	    			System.out.println("laser info:"+laser.getLaserNumber());
	    		}
	    	}
	    	
	    	laser.lockDevice();
	    	robot.addRangeDevice(laser);
	    	laser.unlockDevice();
	    	laser.asyncConnect();   	
	    }else{
	    	ArSonarDevice sonar = new ArSonarDevice();
	    	System.out.println("Sonar device added");
	    	robot.addRangeDevice(sonar);
	    }
	  // laserConn.

	   /**robot.lock();
	   ArModeLaser las = new ArModeLaser(robot, "laser", 'l','L');
	   
	   las.activate();
	   robot.unlock();**/
	    // Add our actions in order. The second argument is the priority, 
	    // with higher priority actions going first, and possibly pre-empting lower
	    // priority actions.
	    /*robot.addAction(recover, 60);
	    robot.addAction(turn, 50);
	    robot.addAction(go, 40);*/
	    robot.addAction(orientar, 70);
	    robot.addAction(avanzar, 60);
	    robot.addAction(esquivar, 90);
	    robot.addAction(recover, 100);
	    // Enable the motors, disable amigobot sounds
	    robot.enableMotors();

	    // Run the robot processing cycle.
	    // 'true' means to return if it loses connection,
	    // after which we exit the program.
	    robot.run(true);
	    
	    
	    Aria.shutdown();
	  }
  
  public static void darDistanciaNominal(ArrayList<Point> points)
  {
	  for(int i=0; i<points.size(); i++)
	  {
		  if(i==0)
		  {
			  disNominal += points.get(i).distance(0, 0);
		  }
		  else
		  {
			  disNominal += points.get(i).distance(points.get(i-1).getPosX(), points.get(i-1).getPosY());
		  }
	  }
  }
  
  public static void agregarMensajeConsola(String mensaje){
	  getInstance().setChanged();
	  getInstance().notifyObservers(mensaje);
  }
  
  public static Proyecto1 getInstance(){
	  if(instance==null){
		  instance = new Proyecto1();
	  }
	  return instance;
  }
}

